/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#include "MuRoMainFrame.h"
#include "Machine.hpp"


using namespace MuRo;

const int MuRoMainFrame::machinesMutex=0;
const wxColour MuRoMainFrame::notEnabledColour=wxColour(200,200,200);
const wxColour MuRoMainFrame::enabledColour=wxColour(0,0,0);

DEFINE_EVENT_TYPE(MY_EVT_UPDATE_DISPLAYS)

BEGIN_EVENT_TABLE(MuRoMainFrame,wxFrame)
  EVT_COMMAND(wxID_ANY, MY_EVT_UPDATE_DISPLAYS, MuRoMainFrame::OnUpdateDisplays)
  EVT_CHILD_FOCUS(MuRoMainFrame::OnChildFocus)
  EVT_MENU(wxID_ANY, MuRoMainFrame::OnMenuEvent)
END_EVENT_TABLE()
    
MuRoMainFrame::MuRoMainFrame( wxWindow* parent)// , CMuRoCore *MuRo)
: MainFrame( parent ),
  CSync(0,1),
  CThread(1),
  helpFrame(this),
  settingsFrame(this),
  glDisplay(this->panel_MainScreen, this),
  selectedMachine(NULL),selectedDevice(NULL),
  MaCICtrlClient(NULL),
  skipSelectionEvent(false)
{
  //dPrint(1,"creating main frame");
  wxBoxSizer *sizer=new wxBoxSizer(wxHORIZONTAL);
  sizer->Add(&glDisplay,1, wxALIGN_CENTRE | wxEXPAND,0);
  panel_MainScreen->SetSizer(sizer);
  
  
  menuItem_Devices=new wxMenuItem(menu_Unit, wxID_ANY, "Devices");//,"",wxITEM_NORMAL,dummySub);
  
  rootItem=treeCtrl_Groups->AddRoot("Root");
  
  rootGroup=new CGroup("Root",this, treeCtrl_Groups, rootItem);
  assert(rootGroup != NULL);

  Layout();
  glDisplay.SetFocus();
  //dPrint(1,"main frame created");
  RunThread();
}

MuRoMainFrame::~MuRoMainFrame()
{
  delete rootGroup;
  if (IsThreadRunning()) {
    SetRequestTermination();
    WaitThread();
  }
}

void MuRoMainFrame::OnExit( wxCommandEvent& event )
{
  
}

void MuRoMainFrame::OnGrid( wxCommandEvent& event )
{
  glDisplay.SetGrid(event.IsChecked());
}

void MuRoMainFrame::OnFollowMode( wxCommandEvent& event )
{
  glDisplay.SetFollowMode(event.IsChecked());
}

void MuRoMainFrame::OnZoomIn( wxCommandEvent& event )
{
  glDisplay.ZoomIn();
}

void MuRoMainFrame::OnZoomOut( wxCommandEvent& event )
{
  glDisplay.ZoomOut();
}

void MuRoMainFrame::OnMiniview( wxCommandEvent& event )
{
  //toggle miniview on and off
  if (scrolledWindow_MiniScreens->IsShown())
  {
    button_MiniView->SetLabel("Show mini view");
    scrolledWindow_MiniScreens->Show(false);
    Layout();
  } else {
    button_MiniView->SetLabel("Hide mini view");
    scrolledWindow_MiniScreens->Show(true);
    Layout();
  }
}

void MuRoMainFrame::OnHelp( wxCommandEvent& event )
{
  //display help
  helpFrame.Show(true);
}


void MuRoMainFrame::OnSettings( wxCommandEvent& event )
{
  //display settings
  settingsFrame.Show(true);


  // do stuff with the frame

}


void MuRoMainFrame::OnOptions( wxCommandEvent& event )
{
  //toggle options frame on and off
  if (panel_Options->IsShown())
  {
    button_Options->SetLabel("Show options");
    panel_Options->Show(false);
    Layout();
  } else {
    button_Options->SetLabel("Hide options");
    panel_Options->Show(true);
    Layout();
  }
}

void MuRoMainFrame::OnPosition( wxCommandEvent& event )
{
  if(selectedMachine)
    selectedMachine->SetOperationMode(KOperModeSetPosition);
}

void MuRoMainFrame::OnObservationMode( wxCommandEvent& event )
{
  if(selectedMachine)
    selectedMachine->SetOperationMode(KOperModeObservation);
}

void MuRoMainFrame::OnManual( wxCommandEvent& event )
{
  if(selectedMachine)
    selectedMachine->SetOperationMode(KOperModeManualDrive);
}

void MuRoMainFrame::OnCoordinateDrive( wxCommandEvent& event )
{
  //dPrint(1,"event..");
  if(selectedMachine)
    selectedMachine->SetOperationMode(KOperModeCoordDrive);
}


void MuRoMainFrame::OnPositionHistory( wxCommandEvent& event )
{
  if(selectedMachine)
    selectedMachine->ShowPositionHistory(event.IsChecked());
}

void MuRoMainFrame::OnRangingHistory( wxCommandEvent& event )
{
  if(selectedMachine)
    selectedMachine->ShowRangingHistory(event.IsChecked());
}

void MuRoMainFrame::OnMachineActivated( wxTreeEvent& event )
{
  wxTreeItemData *d=treeCtrl_Groups->GetItemData(event.GetItem());
  if (d)
    glDisplay.CenterToSelected();
}

void MuRoMainFrame::OnMachineSelected( wxTreeEvent& event )
{
  if (skipSelectionEvent)
  {
    skipSelectionEvent=false;
    event.Skip();
  } else {
    wxTreeItemData *d=treeCtrl_Groups->GetItemData(event.GetItem());
    if (d)
      SelectMachine(((CGroupTreeItemData*)d)->machine->GetName());
  }
}

void MuRoMainFrame::OnDeviceActivated( wxTreeEvent& event )
{
  CDeviceTreeItemData *d=(CDeviceTreeItemData*)(treeCtrl_Services->GetItemData(event.GetItem()));
  selectedDeviceTreeItem=event.GetItem();
  if (d)
  {
    enableDevice(d->device);
  }
  /*
  if (d && d->device->enabled)
  {
    d->device->enabled=false;
    treeCtrl_Services->SetItemTextColour(event.GetItem(), notEnabledColour);
  } else if (d) {
    d->device->enabled=true;
    treeCtrl_Services->SetItemTextColour(event.GetItem(), enabledColour);
  }*/
}


void MuRoMainFrame::OnChildFocus(wxChildFocusEvent &e)
{
  wxWindow *focused=wxWindow::FindFocus();
  
  //if focus is set to other than glcanvas, set it back as glcanvas needs to handle key events
  if (focused!=&glDisplay)
  {
    glDisplay.SetFocus();
  }
}

void MuRoMainFrame::OnMenuEvent(wxCommandEvent &e)
{
  dPrint(1,"menuitem %s, id=%d selected", e.GetString().c_str(), e.GetId());
}

void MuRoMainFrame::OnPositionChoice( wxCommandEvent& event )
{
  if (selectedMachine)
  {
    CChoiceItemData *d=(CChoiceItemData*)(choice_Position->GetClientObject(event.GetInt()));
    selectedMachine->SetReferencePosition((CPositionDevice*)(d->device));
  }
}

void MuRoMainFrame::OnManualChoice( wxCommandEvent& event )
{
  if (selectedMachine)
  {
    CChoiceItemData *d=(CChoiceItemData*)(choice_Manual->GetClientObject(event.GetInt()));
    selectedMachine->SetSpeedCtrlDevice((CSpeedCtrlDevice*)(d->device));
  }
}

void MuRoMainFrame::OnCoordChoice( wxCommandEvent& event )
{
  if (selectedMachine)
  {
    CChoiceItemData *d=(CChoiceItemData*)(choice_CoordDrive->GetClientObject(event.GetInt()));
    selectedMachine->SetCoordinateDriveDevice((CCoordinateDriveDevice*)(d->device));
  }
}

void MuRoMainFrame::OnServiceAssignment(const Gimbo::TServiceAssignmentList &list)
{
  dPrintLCRed(0, "MUUUUUUUUUUUUUUUUUUUUUUUUUUUUUUUUUUUUUUUUUUUUUUUUUUUUUUU");

  dPrint(1,"Service assignments received");
  MaCI::MaCICtrl::TServiceEntryArray maciList;
  gimi::t_serviceList gimiList;
  
  Gimbo::TServiceAssignmentList::const_iterator saIter;
  
  //if MaCICtrl client isnt yet created, create one
  if (!MaCICtrlClient)
  {
    MaCICtrlClient=new MaCI::MaCICtrl::CMaCICtrlClient(GetGimi());
  
    if (MaCICtrlClient->Open()==MaCI::KMaCIOK)
    {
      dPrint(1,"MaCICtrl client opened.");
    } else {
      dPrint(1,"Couldn't open MaCICtrl client");
    }
  }
  
  //perform maci service discovery
  MaCICtrlClient->DiscoverMaCICtrlServices(gimiList, 3000);
  MaCICtrlClient->GetServiceArray(maciList, gimiList, 3000);
  
  for ( saIter=list.begin(); saIter!=list.end(); saIter++ )
  {
    //dPrint(1,"asd");
    MaCI::MaCICtrl::SMaCISL sl(saIter->MaCISL);
  
    CMachine *machine=rootGroup->FindMachine(sl.group);
    if (machine==NULL)
    {
      //dPrint(1,"Creating machine %s",sl.group.c_str());
      //machine with this name doesn't yet exist
      machine=rootGroup->CreateMachine(sl.group);
      //machine=new CMachine(sl.group, this);
      
      AddMachine(machine);
    }
    
    //dPrint(1,"Machine=%p",machine);
    
    switch (saIter->majorType)
    {
      case GIMI_PROTOCOL_MACI_POSITION:
        MaCI::Position::CPositionClient *PositionClient;
        PositionClient=new MaCI::Position::CPositionClient(GetGimi());
        PositionClient->SetDefaultTarget(sl, maciList);
        if (PositionClient->Open()==MaCI::KMaCIOK)
        {
          machine->AddPositionClient(PositionClient);
        } else {
          dPrint(1,"Couldn't open position client.");
          delete PositionClient;
        }
        break;
        
      case GIMI_PROTOCOL_MACI_RANGING:
        MaCI::Ranging::CRangingClient *RangingClient;
        RangingClient=new MaCI::Ranging::CRangingClient(GetGimi());
        RangingClient->SetDefaultTarget(sl, maciList);
        if (RangingClient->Open()==MaCI::KMaCIOK)
        {
          machine->AddRangingClient(RangingClient);
        } else {
          dPrint(1,"Couldn't open ranging client.");
          delete RangingClient;
        }
        break;
        
      case GIMI_PROTOCOL_MACI_SPEEDCTRL:
        MaCI::SpeedCtrl::CSpeedCtrlClient *SpeedCtrlClient;
        SpeedCtrlClient=new MaCI::SpeedCtrl::CSpeedCtrlClient(GetGimi());
        SpeedCtrlClient->SetDefaultTarget(sl, maciList);
        if (SpeedCtrlClient->Open()==MaCI::KMaCIOK)
        {
          machine->AddSpeedCtrlClient(SpeedCtrlClient);
        } else {
          dPrint(1,"Couldn't open speed control client.");
          delete SpeedCtrlClient;
          dPrint(ODTEST, "Deleted the SpeedCtrlClient!");
        }
        break;
        
      case GIMI_PROTOCOL_MACI_COORDINATEDRIVE:
        MaCI::CoordinateDrive::CCoordinateDriveClient *CoordinateDriveClient;
        CoordinateDriveClient=new MaCI::CoordinateDrive::CCoordinateDriveClient(GetGimi());
        CoordinateDriveClient->SetDefaultTarget(sl, maciList);
        if (CoordinateDriveClient->Open()==MaCI::KMaCIOK)
        {
          machine->AddCoordinateDriveClient(CoordinateDriveClient);
        } else {
          dPrint(1,"Couldn't open CoordinateDrive client.");
          delete CoordinateDriveClient;
        }
        break;
        
      case GIMI_PROTOCOL_MACI_ALARM:
        /*MaCI::Alarm::CAlarmClient *AlarmClient;
        AlarmClient=new MaCI::Alarm::CAlarmClient(GetGimi());
        AlarmClient->SetDefaultTarget(sl, maciList);
        if (AlarmClient->Open()==MaCI::KMaCIOK)
        {
          machine->AddAlarmClient(AlarmClient);
        } else {
          dPrint(1,"Couldn't open Alarm client.");
          delete AlarmClient;
          }*/
        break;
        
      case GIMI_PROTOCOL_MACI_ENERGY:
        MaCI::Energy::CEnergyClient *EnergyClient;
        EnergyClient=new MaCI::Energy::CEnergyClient(GetGimi());
        EnergyClient->SetDefaultTarget(sl, maciList);
        if (EnergyClient->Open()==MaCI::KMaCIOK)
        {
          machine->AddEnergyClient(EnergyClient);
        } else {
          dPrint(1,"Couldn't open energy client.");
          delete EnergyClient;
        }
        break;
      case GIMI_PROTOCOL_MACI_MAP:
        MaCI::Map::CMapClient *MapClient;
        MapClient=new MaCI::Map::CMapClient(GetGimi());
        MapClient->SetDefaultTarget(sl, maciList);
        if (MapClient->Open()==MaCI::KMaCIOK)
        {
          machine->AddMapClient(MapClient);
        } else {
          dPrint(1,"Couldn't open map client.");
          delete MapClient;
        }
        break;
        
      default:
        break;
    }
  }
  

}

bool MuRoMainFrame::isAcceptedByMe(const Gimbo::TServiceAssignment *si)
{
  int m=si->majorType;
  return (m==GIMI_PROTOCOL_MACI_RANGING ||
      m==GIMI_PROTOCOL_MACI_SPEEDCTRL ||
      m==GIMI_PROTOCOL_MACI_POSITION ||
      m==GIMI_PROTOCOL_MACI_COORDINATEDRIVE ||
      m==GIMI_PROTOCOL_MACI_MAP ||
      m==GIMI_PROTOCOL_MACI_ALARM ||
      m==GIMI_PROTOCOL_MACI_ENERGY );
  
}

CGLDisplay* MuRoMainFrame::GetGLDisplay()
{
  return &glDisplay;
}

void MuRoMainFrame::DeselectCurrent()
{
  selectMachine(NULL);
}

void MuRoMainFrame::SelectMachine(std::string name)
{
  std::map<std::string, CMachine*>::iterator i;
  i=machines.find(name);
  
  if (i!=machines.end())
  {
    selectMachine(i->second);
  }
}

void MuRoMainFrame::SelectNextMachine(bool previous)
{
  if (!selectedMachine)
  {
    if (machines.size())
    {
      selectMachine(machines.begin()->second);
    } else {
      selectedMachine=NULL;
    }
    return;
  }
  
  std::map<std::string, CMachine*>::iterator i;
  i=machines.find(selectedMachine->GetName());
  
  if (i!=machines.end())
  {
    if (!previous)
    {
      i++;
      if (i==machines.end())
        i=machines.begin();
    } else {
      if (i==machines.begin())
      {
        i=machines.end();
      }
      i--;
    }
    selectMachine(i->second);
  }
}

void MuRoMainFrame::selectMachine(CMachine* machine)
{
  if (selectedMachine==machine)
    return;
  
  if (selectedMachine)
  {
    selectedMachine->Select(false);
    skipSelectionEvent=true;
    treeCtrl_Groups->SelectItem(selectedMachine->GetTreeItem(),false);
    menu_Unit->Remove(menuItem_Devices);
  }
  
  selectedMachine=machine;
  
  selectedDevice=NULL;
  
  if (selectedMachine)
  {
    menubar->EnableTop(menubar->FindMenu("Unit"), true);
    selectedMachine->Select(true);
    
    //menuItem_Devices->SetSubMenu(machine->GetMenu());
    //menu_Unit->Append(menuItem_Devices);
    
    menu_Unit->Enable(ID_MENUITEM_SET_POSITION,selectedMachine->HasPosition());
    menu_Unit->Enable(ID_MENUITEM_MANUAL,selectedMachine->HasManualDrive());
    menu_Unit->Enable(ID_MENUITEM_COORDDRIVE,selectedMachine->HasCoordDrive());
    
    
    if (selectedMachine->GetOperationMode()==KOperModeManualDrive)
    {
      menu_Unit->Check(ID_MENUITEM_MANUAL,true);
    } else if (selectedMachine->GetOperationMode()==KOperModeCoordDrive){
      menu_Unit->Check(ID_MENUITEM_COORDDRIVE,true);
    } else if (selectedMachine->GetOperationMode()==KOperModeObservation){
      menu_Unit->Check(ID_MENUITEM_OBSERVATION,true);
    }
    
    menu_Unit->Check(ID_MENUITEM_POSITION_HISTORY,
                     selectedMachine->IsPositionHistoryShown());
    menu_Unit->Check(ID_MENUITEM_RANGING_HISTORY,
                     selectedMachine->IsRangingHistoryShown());
    
    choice_Manual->Enable(selectedMachine->HasManualDrive());
    choice_CoordDrive->Enable(selectedMachine->HasCoordDrive());
    choice_Position->Enable(selectedMachine->HasPosition());
    
    choice_Manual->Clear();
    choice_CoordDrive->Clear();
    choice_Position->Clear();
    
    if (!treeCtrl_Groups->IsSelected(machine->GetTreeItem()))
    {
      skipSelectionEvent=true;
      treeCtrl_Groups->SelectItem(machine->GetTreeItem());
    }
    //add selected units services to services tree control
    deviceTree::iterator i;
    treeCtrl_Services->DeleteAllItems();
    wxTreeItemId root=treeCtrl_Services->AddRoot("root");
    
    
    machine->LockDeviceMutex(); ///< We lock the machines device mutex
    
    deviceTree& t=machine->GetDeviceTree();
    
    for (i=t.begin(); i!=t.end(); i++)
    {
      deviceVector::iterator ii;
      wxTreeItemId id;
      for (ii=(*i)->begin(); ii!=(*i)->end(); ii++)
      {
        if (!id.IsOk())
        {
          id=treeCtrl_Services->AppendItem(root, 
                    (*ii)->MaCIClient->GetDefaultTargetMaCISL().GetInterface());
        }
        wxTreeItemId devId=treeCtrl_Services->AppendItem(id,
                     (*ii)->MaCIClient->GetDefaultTargetMaCISL().GetInstance(),
                      -1,-1,new CDeviceTreeItemData(*ii));
        if (!(*ii)->enabled)
        {
          treeCtrl_Services->SetItemTextColour(devId, notEnabledColour);
        } else {
          treeCtrl_Services->SetItemTextColour(devId, enabledColour);
        }
        
        //add position and drive devices to according choice boxes
        if ((*ii)->type==GIMI_PROTOCOL_MACI_SPEEDCTRL)
        {
          int n=choice_Manual->Append(
                     (*ii)->MaCIClient->GetDefaultTargetMaCISL().GetInstance(),
                     new CChoiceItemData(*ii));
          if (selectedMachine->GetSpeedCtrlDevice()==(*ii))
            choice_Manual->SetSelection(n);
        } else if ((*ii)->type==GIMI_PROTOCOL_MACI_COORDINATEDRIVE) {
          int n=choice_CoordDrive->Append(
                     (*ii)->MaCIClient->GetDefaultTargetMaCISL().GetInstance(),
                     new CChoiceItemData(*ii));
          if (selectedMachine->GetCoordinateDriveDevice()==(*ii))
            choice_CoordDrive->SetSelection(n);
        } else if ((*ii)->type==GIMI_PROTOCOL_MACI_POSITION) {
          int n=choice_Position->Append(
                     (*ii)->MaCIClient->GetDefaultTargetMaCISL().GetInstance(),
                     new CChoiceItemData(*ii));
          if (selectedMachine->GetReferencePosition()==(*ii))
            choice_Position->SetSelection(n);
        }
        
      }
    }
    
    machine->UnlockDeviceMutex(); ///< We unlock the machines device mutex

  } else {
    treeCtrl_Services->DeleteAllItems();
    menubar->EnableTop(menubar->FindMenu("Unit"), false);
    choice_Manual->Clear();
    choice_CoordDrive->Clear();
    choice_Position->Clear();
  }

}

void MuRoMainFrame::AddMachine(CMachine* machine)
{
  gim::CSync::Lock(machinesMutex);
  machines.insert(std::pair<std::string,CMachine*>(machine->GetName(), machine));
  machine->SetCallback(&glDisplay);
  glDisplay.AddPickable(machine);
  gim::CSync::Unlock(machinesMutex);
}

void MuRoMainFrame::RemoveMachine(CMachine* machine)
{
    gim::CSync::Lock(machinesMutex);
    machines.erase(machine->GetName());
    glDisplay.RemovePickable(machine);
    gim::CSync::Unlock(machinesMutex);
}

void MuRoMainFrame::SetPositionDisplay(float x, float y, float heading)
{
  char s[30];
  sprintf(s,"%.2f , %.2f , %.2f",x,y,heading);
  staticText_Pose->SetLabel(s);
}

int MuRoMainFrame::ThreadFunction(const int aThreadNumber)
{
  while (!IsRequestTermination())
  {
    wxCommandEvent e(MY_EVT_UPDATE_DISPLAYS, GetId());
    AddPendingEvent(e);
    ownSleep_ms(300);
  }
  return 0;
}

void MuRoMainFrame::OnUpdateDisplays(wxCommandEvent& event)
{
  if (selectedMachine) {
    SetPositionDisplay(selectedMachine->GetPositionX(), 
                       selectedMachine->GetPositionY(), 
                           selectedMachine->GetHeading());
  }
}

void MuRoMainFrame::enableDevice(CDevice *device)
{
  if (device->enabled)
  {
    device->enabled=false;
    treeCtrl_Services->SetItemTextColour(selectedDeviceTreeItem, notEnabledColour);
  } else {
    device->enabled=true;
    treeCtrl_Services->SetItemTextColour(selectedDeviceTreeItem, enabledColour);
  }
}
